'''做主要的数学计算'''

import random
import math

def generate_formation_poses(center, r, n):
    """
    center:中心坐标
    r:半径
    n:数量
    返回:n个目标点[(x, y)]
    """
    cx, cy = center
    poses = []
    
    #随机生成N个圆形的目标点
    for i in range(n):
        ang = 2 * math.pi * i / n + random.uniform(-math.pi / 4, math.pi / 4)
        poses.append((cx + r * math.cos(ang),
                        cy + r * math.sin(ang)))
    random.shuffle(poses)#打乱顺序
    print('目标值',poses)
    return poses[:n]


def plan_path(p0, target):
    """
    直飞，返回 (航路点列表, 总长度)
    """
    x0, y0 = p0
    cx, cy = target

    #计算两点之间的距离
    total = math.hypot(cx - x0, cy - y0)
    pts  = [p0]

    # 离散直线（每 50 m 一个点）
    n_leg = max(1, int(total / 50))
    print('n_leg:', n_leg)
    for i in range(1, n_leg + 1):
        t = i / n_leg
        pts.append((x0 + t * (cx - x0), y0 + t * (cy - y0)))
    return pts, total